home *** CD-ROM | disk | FTP | other *** search
- // Heath2.cpp - "driver" function for RARS -
- // Patrick Tierney, May. 1995
- // g2kafka@cdf.toronto.edu
-
- #include <string.h>
- #include <stdlib.h>
- #include <math.h>
- #include "car.h"
-
- // These parameters may be adjusted to get better performance:
- const double h3_csc = 5.400577;
- const double h3_sg = 1.760864;
- const double h3_sd = 0.899998;
- const double h3_ca = 0.089994;
- const double h3_mb = 0.299592;
- const double h3_ea = 0.052816;
- const double h3_sl = 6.503468;
- const double h3_sc = 895.618408;
- const double h3_st = 0.055179;
- const double h3_DF = 88.586960;
- const double h3_CDF = 66.340027;
- const double h3_pow = 11.865677;
- const double h3_er = 90.589142;
- const double h3_tf = 7.906990;
- const double h3_bs = 2.079712;
- const double h3_bf = 2.604871;
- const double h3_as = -0.073984;
- const double h3_vs = 379.745483;
- const double h3_oa = 0.855200;
- const double h3_ss1 = 3.491272;
- const double h3_ss2 = 4.742371;
- const double h3_ss3 = 3.133423;
- const double h3_ss4 = 0.835815;
- const double h3_ss5 = 0.510529;
- const double h3_ss6 = 0.419733;
- const double h3_ct1 = 3.709598;
- const double h3_ct2 = 0.321442;
- const double h3_ct3 = 0.453116;
- const double h3_cv1 = 4.712219;
- const double h3_cv2 = 1.813950;
- const double h3_cv3 = 3.240390;
- const double h3_cv4 = 2.050635;
- const double h3_cw1 = 0.488956;
- const double h3_cw2 = 0.634894;
- const double h3_cw3 = 1.833496;
- const double h3_cw4 = 11.118134;
- const double h3_cw5 = 1.052383;
- const double h3_cw6 = 0.283782;
- const double h3_cw7 = 4.727951;
- const double h3_cw8 = 0.471585;
-
-
- const int h3_PT = 61; // time to stay in passing maneuver, counts
-
- inline double h3_ABS(double arg) {return arg < 0.0 ? -arg: arg;}
-
- double h3_corSpd(double radius)
- {
- if(radius < 0.0) // change sign of negative radius
- radius = -radius;
- else if(radius == 0.0) // This is just insurance, this funtion doesn't
- return(300.0); // make sense when the radius is zero.
- return h3_csc * sqrt(radius+h3_er);
-
- }
-
- double h3_whlSpd(double goal, double present)
- {
- double ws;
-
- if(present > goal + h3_tf* h3_sl) // if way too fast,
- ws = present - h3_bs*h3_sl; // slow down.
- else if(present > goal + 2 * h3_sl) // if too fast,
- ws = present - h3_sl; // slow down.
- else if(present < goal - 2 * h3_sl) // if too slow,
- ws = present + h3_bf*h3_sl; // accelerate.
- else // if quite close,
- ws = (goal + present) / 2; // approach desired speed gently.
-
- return ws;
- }
-
- con_vec Heath2(situation& s)
- {
- const char name[] = "Heath2"; // This is the robot driver's name!
- static int init_flag = 1; // cleared by first call
- double speed; // target speed for cornering, ft/sec
- double speed_next; // target speed for next corner
- con_vec result; // This is what is returned.
- double width; // track width, feet
- double dist;
- double add_rad;
- double the_rad;
- double the_rad2;
- double head;
- double proj_out;
- double ggg;
- double lll;
- int str_type;
- int curve_type;
- double alpha, vc; // components of result
- static double alpha_inc = 0.0; // alpha increment during passing maneuver
- static int counting = 0; // will be set and counting down when passing
-
- if(init_flag) { // first time through, only copy name:
- //pjt_fptr=fopen("zzz.out","wt");
- my_name_is(name);
- init_flag = 0;
- result.alpha = result.vc = 0;
- return result;
- }
-
- if(s.starting){
- result.alpha=h3_as;
- result.vc =s.v+h3_vs;
- return result;
- }
- if(stuck(s.backward, s.v,s.vn, s.to_lft,s.to_rgt, &result.alpha,&result.vc))
- return result;
-
- // Set alpha based on a servo-mechanism approach, trying to stay
- // in the middle of the track, i.e., s.to_left equal to .5 * width:
- width = s.to_lft + s.to_rgt; // find width of track
- curve_type=0;
- if(s.cur_rad==0.0){
- if(s.nex_rad>0.0)
- dist=h3_oa;
- else
- dist=1.0-h3_oa;;
- if(s.cur_len < h3_ss1*width){
- str_type=1;
- if(s.cur_len > h3_ss2*width){
- ggg=s.to_end-width;
- lll=s.cur_len-width;
- }
- else{
- ggg=0.0;
- lll=1.0;
- }
- }
- else{
- str_type=0;
- ggg=s.to_end-h3_ss3*width;
- lll=s.cur_len-h3_ss3*width;
- }
- if((!str_type)&&(ggg>0.0))
- dist=(ggg*(s.to_lft/width - dist) + lll*dist)/lll;
- else
- if((str_type)&&(s.to_end>h3_ss4*width))
- dist=((ggg*(s.to_lft/width - dist) + lll*dist)/lll+0.5)/2.0;
- else
- dist=h3_ss5;
- alpha = h3_ss6*h3_sg * (s.to_lft - dist * width) / width;
- }
- else{
- // check here for a straight-curve
- curve_type=1;
- the_rad=h3_ABS(s.cur_rad/s.cur_len);
- if((s.cur_rad*s.nex_rad<=0.0)&&(width > h3_ct1 * the_rad*(sqrt(2.0/(1.0+cos(s.cur_len)))-1))){
- // curve is fairly straight
- dist=(the_rad/width+0.5)*sqrt((1.0+cos(s.cur_len))/2.0)/
- cos(s.to_end-s.cur_len/2.0)-the_rad/width;
- curve_type=2;
- }
- else
- dist=pow(h3_ABS(2.0*s.to_end/s.cur_len-1.0),h3_pow)*(h3_ss5-h3_ct2)+h3_ct2;
- if(s.cur_rad > 0.0){
- alpha = h3_ct3*h3_sg * (s.to_lft - dist * width) / width;
- alpha+=h3_ca;
- }
- else{
- alpha = h3_ct3*h3_sg * (s.to_lft - (1.0-dist) * width) / width;
- alpha-=h3_ca;
- }
- }
- alpha -= h3_sd * s.vn / s.v; // This is damping, to prevent oscillation
-
- // calculate target speeds for current corner and the next:
- if(curve_type==2)
- speed = h3_corSpd(h3_cv1*s.cur_rad);
- else
- if((curve_type==1)&&(s.cur_rad*s.nex_rad<=0.0)&&(s.cur_len<1.0))
- speed=h3_corSpd(h3_cv2*s.cur_rad);
- else
- speed = h3_corSpd(s.cur_rad); // speed is based on radius
- the_rad2=h3_ABS(s.nex_rad/s.nex_len);
- if((s.nex_rad*s.after_rad<=0.0)&&(width > h3_cv3 * the_rad2*(sqrt(2.0/(1.0+cos(s.nex_len)))-1)))
- speed_next = h3_corSpd(h3_cv4*s.nex_rad);
- else
- speed_next = h3_corSpd(s.nex_rad); // of center line of track.
-
- // now set the tire speed, vc:
- if(s.cur_rad == 0.0){ // If we are on a straightaway,
- if((str_type)&&(h3_ABS(s.nex_rad)>h3_cw1))
- speed_next=h3_cw2*speed_next;
- if(s.v < sqrt(h3_DF*s.to_end+speed_next*speed_next))
- vc = s.v + h3_cw3*h3_sc / s.v; // keep accellerating near full power
- else{ // otherwise,
- if(s.to_end < 2.0*h3_mb * s.cur_len){
- if(s.nex_rad > 0)
- alpha+=h3_ca/h3_cw4;
- else
- alpha-=h3_ca/h3_cw4;
- }
- if (s.v>h3_cw5*speed_next)
- vc=h3_cw6*s.v;
- else
- vc = h3_whlSpd(speed_next, s.v); // brake for next corner
- }
- }
- else{ // If we're in the curve, maintain speed.
- if(s.cur_rad >0.0)
- add_rad=s.to_lft;
- else
- add_rad=s.to_rgt;
- head=(2.0*asin(s.vn/s.v)+alpha)/4.0;
- if(s.to_end<=1.57)
- proj_out=(the_rad+add_rad)/(cos(s.to_end)+tan(head)*sin(s.to_end));
- if((s.to_end > 1.57)||(proj_out>(the_rad+width))||(proj_out<the_rad))
- // if we are far from the next corner, stay at "speed".
- vc = h3_whlSpd(speed, s.v);
- else{ // but when we near the next corner, adjust to "speed_next"
- if(s.cur_rad>0)
- alpha -=h3_ca/h3_cw4;
- else
- alpha +=h3_ca/h3_cw4;
- if (s.nex_rad !=0){
- if(s.v < sqrt(h3_CDF*s.to_end/s.cur_len*h3_ABS(s.cur_rad)+speed_next*speed_next))
- vc=h3_whlSpd(speed_next, s.v);
- else
- vc=h3_cw6*s.v;
- }
- else{
- if(s.nex_len<h3_cw7*width)
- speed_next=h3_cw8*speed_next;
- vc = h3_whlSpd(speed_next, s.v);
- }
- }
- }
- // The passing maneuver:
- if(s.dead_ahead & !counting) { // When first encountering the car ahead:
- counting = h3_PT; // setup the timer,
- if(s.cur_rad !=0.0){
- vc=0.25*s.v;
- counting=0;
- }
- else if((s.to_lft>s.to_rgt)&&(s.nearby[0].rel_xdot>=0))
- alpha_inc=h3_st;
- else if((s.to_lft<s.to_rgt)&&(s.nearby[0].rel_xdot<=0))
- alpha_inc=-h3_st;
- else if(random(10) <2){
- vc=0.25*s.v;
- counting=0;
- }
- else if(s.cur_rad==0.0){
- if(random(29999) < 15000) // choose a right or left maneuver:
- alpha_inc = h3_st;
- else
- alpha_inc = -h3_st;
- }
- else if(s.cur_rad >0.0){
- if(random(29999) < 5000) // choose a right or left maneuver:
- alpha_inc = h3_st;
- else
- alpha_inc = -h3_st;
- }
- else{
- if(random(29999) < 25000) // choose a right or left maneuver:
- alpha_inc = h3_st;
- else
- alpha_inc = -h3_st;
- }
- }
- if(counting) { // If we are still in the passing maneuver,
- alpha += alpha_inc; // change alpha
- --counting; // count down to zero
- }
-
- result.vc = vc; result.alpha = alpha;
- return result;
- }
-
-
-